from Function import *
from math import cos, sin

pi = 3.14159265358979323846264338

class Sigma1(Function):
    def f(self,vec):
        A = np.array([[5,1,0,0.5],[1,4,0.5,0],[0,0.5,3,0],[0.5,0,0,2]])
        return norm(vec)**2/2+np.matmul(np.matmul(np.transpose(vec),A),vec)[0,0]**2/4

class Sigma2(Function):
    def f(self,vec):
        A = np.array([[5,1,0,0.5],[1,4,0.5,0],[0,0.5,3,0],[0.5,0,0,2]])
        return norm(vec)**2/2+np.matmul(np.matmul(np.transpose(vec),A),vec)[0,0]**2*1e4/4

def cosine(x):
    return cos(x/180*pi)

def sine(x):
    return sin(x/180*pi)

F1 = Sigma1()
F2 = Sigma2()
x0 = np.array([[cosine(70)],[sine(70)],[cosine(70)],[sine(70)]])
x1 = np.array([[cosine(50)],[sine(50)],[cosine(50)],[sine(50)]])

print("For sigma = 1, x = x_1:")
ans = Newton_Method(F1,x0)
print("Ans:")
print(ans)
print("with error:",norm(grad(F1,ans)))
ans = pure_Newton_Method(F1,x0)
print("Ans:")
print(ans)
print("with error:",norm(grad(F1,ans)))
print("For sigma = 1, x = x_2:")
ans = Newton_Method(F1,x1)
print("Ans:")
print(ans)
print("with error:",norm(grad(F1,ans)))
ans = pure_Newton_Method(F1,x1)
print("Ans:")
print(ans)
print("with error:",norm(grad(F1,ans)))
print("For sigma = 1e4, x = x_1:")
ans = Newton_Method(F2,x0)
print("Ans:")
print(ans)
print("with error:",norm(grad(F2,ans)))
ans = pure_Newton_Method(F2,x0)
print("Ans:")
print(ans)
print("with error:",norm(grad(F2,ans)))
print("For sigma = 1, x = x_2:")
ans = Newton_Method(F2,x1)
print("Ans:")
print(ans)
print("with error:",norm(grad(F2,ans)))
ans = pure_Newton_Method(F2,x1)
print("Ans:")
print(ans)
print("with error:",norm(grad(F2,ans)))